package pathfinder

import "C"
import (
	"math"
	"src/protoMsg"
	"src/zeus/linmath"
	"src/zeus/logger"
	"unsafe"
)

func NewFindNavmeshCpp(navmesh *Cgo_dtNavMesh, cgo_tonavmesh *Cgo_TempObstaclesNavMesh) IPathFinder {
	if navmesh == nil && cgo_tonavmesh == nil {
		logger.Warn("NewFindNavmeshCpp fail, solo mesh/temp mesh is nill.")
		return nil
	}
	var finder *cgo_dtNavfinder
	if navmesh != nil {
		finder = GetNavFinder(navmesh)
		if finder == nil {
			return nil
		}
	}
	return &FindNavmeshCpp{
		finder:    finder,
		tonavmesh: cgo_tonavmesh,
	}
}

type FindNavmeshCpp struct {
	finder    *cgo_dtNavfinder
	tonavmesh *Cgo_TempObstaclesNavMesh
	/*
		加载mesh的枚举
		0、一般mesh
		1、有动态墙的mesh
		2、有动态墙但不会变的mesh
	*/
	LoadObstacle int32
}

// func (this *FindNavmeshCpp) serverToNav(vec3 *linmath.Vector3) *linmath.Vector3 {
// 	return &linmath.NewVector3((vec3.X + this.AnchorPoint.X) * -0.01, (vec3.Y + this.AnchorPoint.Y) * 0.01, (vec3.Z + this.AnchorPoint.Z) * -0.01)
// }

// func (this *FindNavmeshCpp) navToServer(vec3 *linmath.Vector3) *linmath.Vector3 {
// 	return &linmath.NewVector3(vec3.X*-100 - this.AnchorPoint.X, vec3.Y*100 - this.AnchorPoint.Y, vec3.Z*-100 - this.AnchorPoint.Z)
// }

/*
这两个函数是用来做单位装换的。
服务器是米为单位，然后根据客户端的单位做转换逻辑
*/

func (this *FindNavmeshCpp) serverToNav(vec3 *linmath.Vector3) *linmath.Vector3 {
	return &linmath.Vector3{
		Vector3: protoMsg.Vector3{X: (vec3.X) * 1, Y: (vec3.Y) * 1, Z: (vec3.Z) * 1},
	}
	// return &linmath.Vector3{
	// 	Vector3: protoMsg.Vector3{X: (vec3.X) * -0.01, Y: (vec3.Y) * 0.01, Z: (vec3.Z) * -0.01},
	// }
}

func (this *FindNavmeshCpp) navToServer(vec3 *linmath.Vector3) *linmath.Vector3 {
	return &linmath.Vector3{
		Vector3: protoMsg.Vector3{X: vec3.X * 1, Y: vec3.Y * 1, Z: vec3.Z * 1},
	}
	// return &linmath.Vector3{
	// 	Vector3: protoMsg.Vector3{X: vec3.X * -100, Y: vec3.Y * 100, Z: vec3.Z * -100},
	// }
}

func (this *FindNavmeshCpp) FindPath(stpos, edpos linmath.Vector3) ([]linmath.Vector3, bool) {

	spos := this.serverToNav(&stpos)
	epos := this.serverToNav(&edpos)

	count := int32(0)
	var floats *C.float

	if this.tonavmesh == nil {
		if this.finder == nil {
			return nil, false
		}
		if floats = FindNavPath(this.finder, spos, epos, &count); count == 0 {
			return nil, false
		}
	} else {
		if floats = FindNavPathTO(this.tonavmesh, spos, epos, &count); count == 0 {
			return nil, false
		}
	}

	path := make([]linmath.Vector3, count, count)
	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(linmath.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		path[i] = *this.navToServer(linVec)
	}
	logger.Debug(logger.LogKey_MoveSys, "[FindNavmeshCpp] FindPath 寻路成功 路点个数 ", count)
	return path, true
}

// GetNear 获取某个点附近的合法位置
// 参数：
//
//	params 检查点 *linmath.Vector3 类型
//
// 返回值：
//
//	pos 路点，注意第一个点就是输入的起始点
//	result 是否能成功
func (this *FindNavmeshCpp) GetNear(srcPos *linmath.Vector3) (*linmath.Vector3, bool) {
	if srcPos == nil {
		logger.Error("GetNear params not match")
		return nil, false
	}
	spos := this.serverToNav(srcPos)
	epos := linmath.NewVector3(0, 0, 0)

	var floats *C.float
	var count int32

	if this.tonavmesh == nil {
		if this.finder == nil {
			return nil, false
		}
		if floats = CalcNavNear(this.finder, spos, &count); count == 0 {
			return nil, false
		}
	} else {

		if floats = CalcNavNearTO(this.tonavmesh, spos, &count); count == 0 {
			return nil, false
		}
	}

	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(linmath.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		epos = *this.navToServer(linVec)
	}
	// logger.Debug(logger.LogKey_MoveSys, "[FindNavmeshCpp] GetNear spos ", srcPos, "epos ", epos)
	return &epos, true
}

// CheckWall 按照100单位步长检查墙
// 墙的定义点位高程超过500单位
func (this *FindNavmeshCpp) CheckWall(srcPos, dstPos linmath.Vector3) (path []linmath.Vector3, bHaveWall bool) {
	bHaveWall = false

	srcPos.Y = dstPos.Y
	logger.Debug(logger.LogKey_MoveSys, "CheckWall srcPos ", srcPos, " dstPos ", dstPos)

	dir := dstPos.Sub(srcPos)
	dst := float64(dir.Len())
	dir.Normalize()

	minDst := 100.0

	stepCount := int(math.Ceil(dst / minDst))

	tmpPos := *linmath.Vector3_Zero()
	for i := 0; i < stepCount; i++ {
		curDst := float32(float64(i+1) * minDst)
		tmpPos = srcPos.Add(dir.Mul(curDst))
		if nearPos, ok := this.GetNear(&tmpPos); ok {
			if !nearPos.IsEqualXZ(tmpPos) {
				path = append(path, *nearPos)
				logger.Debug(logger.LogKey_MoveSys, "nearPos ", *nearPos)
				bHaveWall = true
				return
			}
			// if math.Abs(float64(nearPos.Y-dstPos.Y)) > 2000.0 {

			// 	logger.Debug("nearPos too high ", *nearPos)
			// 	bHaveWall = true
			// 	return
			// }
		} else {
			// path = append(path, *nearPos)
			logger.Debug(logger.LogKey_MoveSys, "nearPos not find near tmpPos ", tmpPos)
			bHaveWall = true
			return
		}
	}
	return
}

// Raycast 射线检测
func (this *FindNavmeshCpp) Raycast(params interface{}) (*linmath.Vector3, bool) {

	navmesh_params, ok := params.(NavmeshParams)
	if !ok {
		logger.Error("[FindNavmeshCpp] Raycast params not match")
		return nil, false
	}
	spos := this.serverToNav(&navmesh_params.Start)
	epos := this.serverToNav(&navmesh_params.End)

	var floats *C.float
	var count int32

	if this.tonavmesh == nil {
		if this.finder == nil {
			return nil, false
		}
		if floats = CalcRaycast(this.finder, spos, epos, &count); count == 0 {
			return nil, false
		}
	} else {
		if floats = CalcRaycastTO(this.tonavmesh, spos, epos, &count); count == 0 {
			return nil, false
		}
	}

	path := make([]linmath.Vector3, count, count)
	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(linmath.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		path[i] = *this.navToServer(linVec)
	}
	return &(path[0]), true
}

func (this *FindNavmeshCpp) RaycastEx(srcPos, dstPos linmath.Vector3) (*linmath.Vector3, bool) {

	params := NavmeshParams{srcPos, dstPos}
	return this.Raycast(params)
}

// 动态阻挡相关
// 增加可旋转的Box阻挡，先强行使用AABB来实现
// OBB当旋转角度在这些区间（[5°,5°] [85°,95°] [175,-175°] [-95°,-85°]），将会强制纠成AABB；
func (this *FindNavmeshCpp) CreateObstacleOBBBox(doorName string, centPos, halfPos linmath.Vector3, YawDegree int32) {

	if this.tonavmesh == nil {
		logger.Warn("[FindNavmeshCpp] CreateObstacleOBBBox this.tonavmesh == nil")
		return
	}
	// centPos = linmath.PosClient2Nav(&centPos)
	// hPos := linmath.NewVector3(linmath.Client2NavScale(halfPos.X), linmath.Client2NavScale(halfPos.Z), linmath.Client2NavScale(halfPos.Y))
	angle := linmath.AngleTo180(float32(YawDegree))
	yawRedian := linmath.AngleToRadian(angle)
	AddObbBoxTO(this.tonavmesh, doorName, &centPos, &halfPos, float32(yawRedian))
	UpdateTO(this.tonavmesh)
}

// 增加圆柱体阻挡
func (this *FindNavmeshCpp) CreateObstacleCylinder(doorName string, centPos linmath.Vector3, radius, height int32) {
	if this.tonavmesh == nil {
		logger.Warn("[FindNavmeshCpp] CreateObstacleCylinder this.tonavmesh == nil")
		return
	}
	centPos = linmath.PosClient2Nav(&centPos)
	fradius := linmath.Client2NavScale(float32(radius))
	fheight := linmath.Client2NavScale(float32(height))
	AddCyinderTO(this.tonavmesh, doorName, &centPos, fradius, fheight)
	UpdateTO(this.tonavmesh)
}

// 清理掉某个门
func (this *FindNavmeshCpp) RemoveObstacle(doorName string) {
	if this.tonavmesh == nil {
		logger.Warn("[FindNavmeshCpp] RemoveObstacle this.tonavmesh == nil")
		return
	}
	RemoveTOByTag(this.tonavmesh, doorName)
	UpdateTO(this.tonavmesh)
}

// 清理全部的门
func (this *FindNavmeshCpp) RemoveAllObstacle() {
	if this.tonavmesh == nil {
		logger.Warn("[FindNavmeshCpp] RemoveAllObstacle this.tonavmesh == nil")
		return
	}
	RemoveAllTO(this.tonavmesh)
}

// 更新阻挡信息
func (this *FindNavmeshCpp) UpdateObstacle() {
	if this.tonavmesh == nil {
		return
	}
	UpdateTO(this.tonavmesh)
}

func (this *FindNavmeshCpp) Destroy() {
	if this.finder != nil {
		FreeNavMesh(this.finder)
		this.finder = nil
	}
	if this.tonavmesh != nil {
		FreeToNavMesh(this.tonavmesh)
		// Free(unsafe.Pointer(this.tonavmesh))
		this.tonavmesh = nil
	}
}

func (this *FindNavmeshCpp) MeshCheckRushPath(srcPos, stpos linmath.Vector3) linmath.Vector3 {
	if tmpPos, ok := this.RaycastEx(srcPos, stpos); !ok {
		logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 未能检测到墙直接通过 ", stpos)
		return stpos
	} else {
		if tmpPos.IsEqualXZ(srcPos) {
			if path, ok := this.FindPath(srcPos, stpos); ok {
				if len(path) == 2 {
					*tmpPos = path[1]
					logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 检测到墙 再次尝试走到墙根 ", path[1])
				}
			}
		} else {
			logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 检测到墙 ", tmpPos)
		}
		stpos = *tmpPos
		return stpos
	}
}

func TestPathFind() {

	// navmesh := GetNavMesh("D:/work/trunk/new_project_src/src/res/space/DemoLevel/level.bin")
	// finder := GetNavFinder(navmesh)

	// spos := linmath.NewVector3(-8.21916, 5.5, -83.591866)
	// epos := linmath.NewVector3(-6.5794773, 5.5, -72.27619)

	// FindNavPath(finder, &spos, &epos)
	// count := int32(0)
	// floats := GetNavPath(finder, &count)

	// retPtr := unsafe.Pointer((*C.float)(floats))
	// size := unsafe.Sizeof(linmath.Vector3{})
	// logger.Debug("[FindNavmeshCpp] FindPath count ", count)
	// for i := 0; i < int(count); i++ {
	// 	linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
	// 	logger.Debug("[FindNavmeshCpp] i ", i, " pos ", linVec)
	// }

	// navmesh := GetNavMesh("D:/work/trunk/new_project_src/src/res/space/DemoLevel/level.bin")
	// finder := GetNavFinder(navmesh)
	// spos := linmath.NewVector3(-8.21916, 5.5, -83.591866)
	// epos := linmath.NewVector3(-6.5794773, 5.5, -72.27619)

	// logger.Debug(logger.LogKey_MoveSys, "[FindNavmeshCpp] spos ", spos, " epos ", epos)
	// count := int32(0)
	// var floats *C.float

	// floats = GetNavPath(finder, &count)

	// retPtr := unsafe.Pointer((*C.float)(floats))
	// size := unsafe.Sizeof(linmath.Vector3{})
	// logger.Debug(logger.LogKey_MoveSys, "[FindNavmeshCpp] FindPath count ", count)
	// for i := 0; i < int(count); i++ {
	// 	linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
	// 	logger.Debug(logger.LogKey_MoveSys, "[FindNavmeshCpp] i ", i, " pos ", linVec)
	// }
}
